#include "gmr.h"
#include "MathLib.h"

/**
 * \defgroup Gesture
 */


/**
 * Wrapper class to learn and replay one gesture.
 *
 * \ingroup Gesture
 *
 * This class may be used with an arbitrary number of dimensions (not
 * necessariliy 3..). The demonstrations are provided stored in array
 * of Matrix objects via the setdemos methods. You must call setdemos
 * once per reference object. 
 * 
 * It is possible to provide demonstration in both task and gesture
 * space to retrieve a reference posture during the reproduction
 * phase. 
 *
 * A trained gesture can be saved to / load from a file with
 * saveParams/loadParams methods, and the class provide several
 * different methods for the reproduction phase. 
 *
 * Basic Work cycle : 
 * @code
 *  g = Gesture(2) // init a Gesture with 2 reference objects 
 *  
 *  // stores demonstration relative to each objects  
 *  g.setdemos(trajectories_relative_to_1,joints_angles,3,1) 
 *  g.setdemos(trajectories_relative_to_2,3,2)
 *
 *  g.train()
 *  
 *  g.init_trajectory()
 *  g.init_alpha()
 *  while(!finished)
 *    {
 *        position += g.trajectory_step_hmm(position,{object1,object2});
 *        reference_posture = g.getReferencePosture();
 *    }
 * @endcode
 *
 */
 
class Gesture
{
 public :
  
  /**
   * create a Gesture objects. 
   * \parama n_ref  numbers of reference objects ( *1* OR *2*)
   * \param nstates  number of GMM state for encoding the Gesture 
   * \param n_resample the demos are resampled to this number before training  
   * \param nbData  nb of states for generating reference trajectories
   */
   
  Gesture(int n_ref,int nstates=4,int nbData=100,int n_resample=100);
  
  /**
   * save Gesture Model to file. 
   *
   * if filename_joints is set save also the joints Model the models
   * are saved under the filename%02d according to each reference
   * objects
   */

  void saveParams(char filename[],char filename_joints[]);
  void saveParams(char filename[]);

  /**
   * load a saved reference object.
   * the n_ref MUST be correctly set before calling this 
   */

  void loadParams(char filename[],char filename_joints[]);
  void loadParams(char filename[]);
  

  /**
   * store demos priors to learning. 
   *
   * \param current_ref is the index of the objects the demos are 
   * \param ndemos are the numbers of demonstrations 
   * must be called once per reference object 
   */
  void setdemos(Matrix * raw,Matrix * joints, int ndemos, int current_ref);
  void setdemos(Matrix * raw,int ndemos, int current_ref);
  
  /**
   * get back all resized demos DEPRECATED
   */
  void getalldemos(Matrix * datasets);

  /**
   * train all the GMM models .. 
   *
   * Must be call once all the demos are stored and before 
   * generating the reference trajectories and getting back
   * controllers 
   */
  void train();

  /**
   * init reference trajectories. 
   *
   * Must be called before the trajectory_step_* functions 
   */
  void init_trajectory();
  
  /**
   * save the references trajectories to text files. 
   * In the absolute referential if the Homogenous h Matrices 
   * are provided 
   */
  void save_reference_trajectories(void);
  void save_reference_trajectories(Matrix4 * h);
  

  /**
   * returns speed considering current position
   *
   * For comparison purposes .. only work with one frame of reference,
   *and need to call init_trajectory() method first 
   */

  Vector3 trajectory_step_nohmm(Vector3 CurrentPosition);

  /**
   * Get a speed vector from current position, and current objects positions
   * if pstate is -1 : try to first estimate currrent state
   * (Fast forward algorithm for HMM .. )
   */
  Vector3 trajectory_step(int&  pstate, Vector3 currentPosition,
			 Matrix4 * objectsPosition);

  /**
   * Initialize forward hmm algorithm 
   *
   * Set up the alpha probabilities must be called before the first
   * call to trajectory_step_hmm
   */
  void init_alpha(void);

  /**
   * get a speed from current position 
   * 
   * uses HMM forward algorithm to compute weight for each reference
   * state, and retrieve a speed vector.  You must call
   * init_trajectory() and init_alpha() to initialize reference
   * trajectories and forward probabilities before this one.
   *
   *
   * \param CurrentPosition Current position in the absolute referential 
   * \param objectsPos array of homogeneous matrices describing the referential of each
   * landmarks 
   */
  Vector3 trajectory_step_hmm(Vector3 CurrentPosition);
  Vector3 trajectory_step_hmm(Vector3 CurrentPosition,Matrix4 * objectsPos);
 

  
  void init_dynamics(Vector CurrentPosition);
  
  /**
   * get a speed from current position
   *
   * uses the 2nd order controller to get speed from current position
   * this method is still experimental and not well implemented : it
   * does not use hmm, so is sensitive to loops, and does not handle
   * several landmarks.  You must call init_trajectory() and
   * init_dynamics() before the first call to this method.
   */ 
  Vector trajectory_step_dyn(Vector CurrentPosition);
  

  /**
   * get the Current Reference posture 
   *
   * if the Gesture has been thaught with joints values ( setdemos()
   * methods), uses the forward probabilites prealably computed with
   * the trajectory_step_hmm method to compute a reference posture.  
   */ 
  Vector getReferencePosture(void);
 

  GaussianMixture ** gmms;
  Matrix * means;

 protected :
  int nObjects;
  int demo_has_time;

  int nbVarPos;
  int nbJoints;
  int nbData;
  int nbVar;
  int nstates;
  int n_resample;

  Vector previous_position;
  Vector previous_speed;
  
  
  Matrix ** sigmas;

  Matrix outData;
  Matrix * outSigma;
  Matrix dataset;
  Matrix joints_dataset;
  Matrix outJoints;
  Matrix * joints_demos;

  GaussianMixture g_joints; // to encode joints angles .. 
 

  Matrix ** demos;
  int * ndemos; // nbs of demos considering reference i 
  Matrix add_time(Matrix& demo);
  Matrix computeVelocity_notime(Matrix posData);
  Matrix computeVelocity(Matrix posData);
  double * alpha;
};

/**
 * Gaussian products helper functions
 *
 * used to combine models defined relatively to each target object
 */
int GaussProduct(Vector Mu1,Matrix Sigma1,Vector Mu2,Matrix Sigma2,Vector& Mu, Matrix& Sigma);

/**
 * Batch Gaussian Product
 *
 * \param Mu1 matrix of n (rows) means Vectors
 * \param Sigma1 array of n Covariances matrices 
 */
int GaussProduct(Matrix Mu1, Matrix * Sigma1,
		 Matrix Mu2, Matrix * Sigma2,
		 Matrix& Mu, Matrix * Sigma);


/**
 * \mainpage 
 *
 *  This the Gesture package doxygen generated documentation : 
 *  - if you want to use Kinematics computation : see IKModel class 
 *  - if you want to learn and retrieve Gestures : see the  Gesture class
 *  - The Gesture class heavily relies on Gaussian Mixtures Models, that are implemented 
 *  by the GaussianMixture models. 
 *
 * Florent D'halluin florent.dhalluin@epfl.ch
 * 
 */



